﻿//
// RobotSimulator.cs
//
// Authors:
//    Claus Jørgensen <10229@iha.dk>
//    Jens Bloch Christensen <09584@iha.dk>
//    Jonas Daugaard Møller <08814@iha.dk>
//
using System;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Threading.Tasks;
using System.Threading;
using System.Collections.Generic;
using System.Collections.Concurrent;

namespace RSD.Robot
{
#pragma warning disable 67

    /// <summary>
    /// Simulator of the methods required for using the Intelitek USBC-Controller.
    /// </summary>
    public class RobotSimulator : USBC, IRobotController
    {
        #region Callbacks

        /// <summary>
        /// Initialization Complete callback.
        /// </summary>
        private InitializationCompleteCallback initializationComplete;

        /// <summary>
        /// Error callback.
        /// </summary>
        private ErrorCallback error;

        /// <summary>
        /// Motion Started callback.
        /// </summary>
        private MovementCallback motionStarted;

        /// <summary>
        /// Motion Ended callback.
        /// </summary>
        private MovementCallback motionEnded;

        /// <summary>
        /// Control Status Changed callback.
        /// </summary>
        private WatchControlCallback controlStatusChanged;

        /// <summary>
        /// Homing Status Changed callback.
        /// </summary>
        private HomingNotifierCallback homingStatusChanged;

        /// <summary>
        /// Join Status Changed callback.
        /// </summary>
        private WatchJointCallback jointStatusChanged;

        /// <summary>
        /// Digital Input Recieved callback.
        /// </summary>
        private IOMonitorCallback digitalInputReceived;

        /// <summary>
        /// Digital Output Recieved callback.
        /// </summary>
        private IOMonitorCallback digitalOutputReceived;

        #endregion

        #region Queue Members

        /// <summary>
        /// Internal queue of robot actions.
        /// </summary>
        private Queue<Action> actionQueue = new Queue<Action>();

        /// <summary>
        /// Indicates whether the robot is paused.
        /// </summary>
        private bool isPaused = false;

        #endregion

        #region Constructor and Destructor

        /// <summary>
        /// Initializes a new instance of the <see cref="T:RobotSimulator"/> class.
        /// </summary>
        public RobotSimulator()
        {
            this.initializationComplete = new InitializationCompleteCallback(OnInitializationCompleted);
            this.error = new ErrorCallback(OnError);
            this.motionStarted = new MovementCallback(OnMotionStarted);
            this.motionEnded = new MovementCallback(OnMotionEnded);
            this.controlStatusChanged = new WatchControlCallback(OnControlStatusChanged);
            this.homingStatusChanged = new HomingNotifierCallback(OnHomingStatusChanged);
            this.jointStatusChanged = new WatchJointCallback(OnJointStatusChanged);
            this.digitalInputReceived = new IOMonitorCallback(OnDigitalInputReceived);
            this.digitalOutputReceived = new IOMonitorCallback(OnDigitalOutputReceived);
        }

        /// <summary>
        /// Finalizes an instance of the <see cref="T:RobotSimulator"/> class.
        /// </summary>
        ~RobotSimulator()
        {
            Dispose(false);
        }

        #endregion

        #region IDisposable Members

        /// <summary>
        /// Releases all resources used by the <see cref="T:USBC"/> object.
        /// </summary>
        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Releases all resources used by the <see cref="T:USBC"/> object.
        /// </summary>
        /// <param name="disposing">True if the call was from the user's code, or false if called from a finalizer.</param>
        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                try
                {
                    Close();
                }
                catch (DllNotFoundException e)
                {
                    Trace.TraceError(e.Message);
                }
            }
        }

        #endregion

        #region IRobotController Members

        /// <summary>
        /// Gets or sets the folder which contains the robot configuration files.
        /// </summary>
        public string ParameterFolder
        {
            get
            {
                throw new NotImplementedException();
            }
            set
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Gets the robot version number.
        /// </summary>
        public uint Version
        {
            get
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Gets a value indicating whether the emergency button has activated or not.
        /// </summary>
        public bool IsEmergency
        {
            get
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Gets a value indicating whether the robot is in teaching mode or not.
        /// </summary>
        public bool IsInTeachingMode
        {
            get
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Gets a value indicating whether the robot is online.
        /// </summary>
        public bool IsOnline
        {
            get
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Gets the width between the grippers fingers.
        /// </summary>
        public Tuple<short, short> Jaw
        {
            get
            {
                var random = new Random();

                short perc = (short)random.Next(0, 100);
                short metricValue = (short)random.Next(0, 100);

                return Tuple.Create(perc, metricValue);
            }
        }

        /// <summary>
        /// Gets the configuration for the connected robot.
        /// </summary>
        public USBCConfiguration Configuration
        {
            get
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer, using the default <see cref="SystemType"/>.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Initialization(InitializationMode mode)
        {
            return Initialization(mode, SystemType.Default);
        }

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <param name="systemType">System type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Initialization(InitializationMode mode, SystemType systemType)
        {
            if (initializationComplete == null || error == null)
            {
                throw new InvalidOperationException();
            }

            bool initialized = true;

            if (initialized)
            {
                MonitorDigitalInput();
                MonitorDigitalOutput();
                MonitorControlStatus();
                MonitorHomingStatus();
                MonitorJointStatus();
                MonitorMotion();
            }

            OnInitializationCompleted(new USBCConfiguration());

            return initialized;
        }

        /// <summary>
        /// Closes the connection to the USBC Controller.
        /// </summary>
        public void Close()
        {
        }

        /// <summary>
        /// Monitors the digital input.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorDigitalInput()
        {
            if (digitalInputReceived == null)
            {
                throw new InvalidOperationException();
            }

            return true;
        }

        /// <summary>
        /// Monitors the digital output.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorDigitalOutput()
        {
            if (digitalOutputReceived == null)
            {
                throw new InvalidOperationException();
            }

            return true;
        }

        /// <summary>
        /// Gets the digital input.
        /// </summary>
        /// <returns>the digital input.</returns>
        public uint GetDigitalInput()
        {
            return 1;
        }

        /// <summary>
        /// Gets the digital output.
        /// </summary>
        /// <returns>the digital output.</returns>
        public uint GetDigitalOutput()
        {
            return 1;
        }

        /// <summary>
        /// Monitors the robots movement.
        /// </summary>
        public void MonitorMotion()
        {
            if (motionStarted == null || motionEnded == null)
            {
                throw new InvalidOperationException();
            }            
        }

        /// <summary>
        /// Sets the time future movement should take, for the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="time">Movement time.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetTime(Axis axis, TimeSpan time)
        {
            return true;
        }

        /// <summary>
        /// Sets the speed future movement should take, for the given axis.
        /// </summary>
        /// <remarks>
        /// A <paramref name="speed"/> of 0 will cause the movement to stop.
        /// </remarks>
        /// <param name="axis">Axis to move.</param>
        /// <param name="speed">Movement speed (0-10).</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetSpeed(Axis axis, int speed)
        {
            if (speed <= 0 || speed > 10)
            {
                throw new ArgumentException("Speed must be between 0 and 10");
            }

            return true;
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveJoint(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            OnMotionStarted((byte)Axis.Robot);
            OnMotionEnded((byte)Axis.Robot);

            return true;
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveJoint(Vector vectorA, Vector vectorB)
        {
            if (vectorB == null)
            {
                throw new ArgumentNullException("vectorA");
            }

            if (vectorA == null)
            {
                throw new ArgumentNullException("vectorB");
            }

            OnMotionStarted((byte)Axis.Robot);
            OnMotionEnded((byte)Axis.Robot);

            return true;
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveLinear(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveLinear(Vector vectorA, Vector vectorB)
        {
            if (vectorA == null)
            {
                throw new ArgumentNullException("vectorA");
            }

            if (vectorB == null)
            {
                throw new ArgumentNullException("vectorB");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Moves the conveyor belt.
        /// </summary>
        /// <param name="velocity">Velocity of the conveyor belt in percent.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveConveyorBelt(int velocity)
        {
            if (velocity < 0 || velocity > 100)
            {
                throw new ArgumentException("velocity must be between 0 and 100");
            }

            OnMotionStarted((byte)Axis.ConveyorBelt);
            OnMotionEnded((byte)Axis.ConveyorBelt);

            return true;
        }

        /// <summary>
        /// Opens the gripper.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool OpenGripper()
        {
            OnMotionEnded((byte)Axis.Gripper);

            return true;
        }

        /// <summary>
        /// Closes the gripper.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool CloseGripper()
        {
            OnMotionEnded((byte)Axis.Gripper);

            return true;
        }

        /// <summary>
        /// Stops all movement on the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool StopMovement(Axis axis)
        {
            return true;
        }

        /// <summary>
        /// Sets the default velocity of the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="velocity">Velocity in percent.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetVelocity(Axis axis, short velocity)
        {
            if (velocity < 0 || velocity > 100)
            {
                throw new ArgumentException("velocity must be between 0 and 100");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Turns control on or off for the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="isOn">true to turn on, false to turn off.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetControl(Axis axis, bool isOn)
        {
            return true;
        }

        /// <summary>
        /// Monitors the control status.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorControlStatus()
        {
            if (controlStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return true;
        }

        /// <summary>
        /// Monitors the homing process.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorHomingStatus()
        {
            if (homingStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return true;
        }

        /// <summary>
        /// Homes the robot.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Home(Axis axis)
        {
            if (homingStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            OnHomingStatusChanged((char)Axis.Robot);

            return true;
        }

        /// <summary>
        /// Sets the home position to the current position.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetHome(Axis axis)
        {
            return true;
        }

        /// <summary>
        /// Monitors the values for each joint on the robot.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorJointStatus()
        {
            if (jointStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return true;
        }

        /// <summary>
        /// Loads points into the robots memory from a file.
        /// </summary>
        /// <param name="fileName">The file to open.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool LoadPointsFromFile(string fileName)
        {
            if (string.IsNullOrEmpty(fileName))
            {
                throw new ArgumentException("fileName cannot be null or empty");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Defines a vector in the robots memory.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="vectorName">Name of the vector.</param>
        /// <returns>
        /// a vector with the given vectorname; otherwise, null.
        /// </returns>
        public Vector DefineVector(Axis axis, string vectorName)
        {
            if (string.IsNullOrEmpty(vectorName))
            {
                throw new ArgumentException("vectorName cannot be null or empty.");
            }

            if (axis != Axis.Robot || axis == Axis.All)
            {
                throw new ArgumentException("axis must be either Axis.Robot or Axis.All");
            }

            short dimension = 1;

            return new Vector(axis, vectorName, dimension);
        }

        /// <summary>
        /// Renames a vector in the robots memory.
        /// </summary>
        /// <param name="oldName">Old vector name.</param>
        /// <param name="newName">New vector name.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool RenameVector(string oldName, string newName)
        {
            if (string.IsNullOrEmpty(oldName))
            {
                throw new ArgumentException("oldName cannot be null or empty");
            }

            if (string.IsNullOrEmpty(newName))
            {
                throw new ArgumentException("newName cannot be null or empty");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Deletes all points from a vector in the robots memory.
        /// </summary>
        /// <param name="vector">Vector to delete.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool DeleteVector(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Deletes a vector point in the robots memory.
        /// </summary>
        /// <param name="vector">Vector to delete a point from.</param>
        /// <param name="point">Point to remove from the given vector.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool DeleteVector(Vector vector, short point)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Saves the current position as a point in memory.
        /// </summary>
        /// <param name="vector">Vector to save the position to.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SaveCurrentLocation(Vector vector, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            throw new NotImplementedException();
        }

        /// <summary>
        /// Sets the joints.
        /// </summary>
        /// <param name="vector">The vector.</param>
        /// <param name="position">The position.</param>
        /// <param name="pointType">Type of the point.</param>
        /// <returns></returns>
        public bool SetJoints(Vector vector, Position position, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            vector.Position = position;

            return true;
        }

        /// <summary>
        /// Teaches a robot the location of a named vector.
        /// </summary>
        /// <param name="vector">Vector to teach.</param>
        /// <param name="position">XYZ position to associate the vector with.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Teach(Vector vector, Position position, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            vector.Position = position;

            return true;
        }

        /// <summary>
        /// Gets the point information for a given vector and point-number.
        /// </summary>
        /// <param name="vector">Vector to get point information for.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>point information for the given location.</returns>
        public Position GetPointInfo(Vector vector, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            int[] enc = new int[TotalAxes];
            int[] xyz = new int[TotalAxes];

            throw new NotImplementedException();
        }

        /// <summary>
        /// Gets the current position of the robot.
        /// </summary>
        /// <returns>the robots current position.</returns>
        public Position GetCurrentPosition()
        {
            int[] enc = new int[TotalAxes];
            int[] joint = new int[TotalAxes];
            int[] xyz = new int[TotalAxes];

            throw new NotImplementedException();
        }

        /// <summary>
        /// Enqueues a position.
        /// </summary>
        /// <param name="position">Position with joint coordinates.</param>
        /// <param name="pointType">Type of the position points.</param>
        public void MoveToPosition(Position position, PointType pointType)
        {
            string vectorName = Guid.NewGuid().ToString();

            if (string.IsNullOrEmpty(vectorName))
            {
                throw new InvalidOperationException("Error when creating a GUID as vector name.");
            }
            
            var vector = DefineVector(Axis.Robot, vectorName);
            vector.Position = position;

            switch (pointType)
            {
                case PointType.RelativeJointCoordinate:
                case PointType.AbsoluteJointCoordinate:
                    SetJoints(vector, vector.Position, pointType);
                    break;
                case PointType.AbsoluteXYZCoordinateA:
                case PointType.AbsoluteXYZCoordinateB:
                case PointType.RelativeXYZCoordinate:
                    Teach(vector, vector.Position, pointType);
                    break;
            }

            MoveJoint(vector);
        }

        /// <summary>
        /// Enqueues any RobotController action.
        /// </summary>
        /// <param name="item">Action delegate.</param>
        public void EnqueueAction(Action<IRobotController> item)
        {
            actionQueue.Enqueue(() => item(this));
        }

        /// <summary>
        /// Waits for conveyor belt to send input.
        /// </summary>
        public void WaitForConveyorBelt()
        {
            OnDigitalInputReceived(1);
        }

        /// <summary>
        /// Triggers the ActionCompleted event.
        /// </summary>
        public void TriggerActionCompleted()
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Executes the movement queue.
        /// </summary>
        public void RunActionQueue()
        {
            MotionStarted += RobotControllerQueue_MotionStarted;
            MotionEnded += RobotControllerQueue_MotionEnded;
            InitializationCompleted += RobotSimulator_InitializationCompleted;
            HomingStatusChanged += RobotSimulator_HomingStatusChanged;
            DigitalInputReceived += RobotSimulator_DigitalInputReceived;

            while (actionQueue.Count > 0)
            {
                if (!isPaused)
                {
                    isPaused = true;

                    var action = actionQueue.Dequeue();
                    action();
                }
            }

            DigitalInputReceived -= RobotSimulator_DigitalInputReceived;
            InitializationCompleted -= RobotSimulator_InitializationCompleted;
            HomingStatusChanged -= RobotSimulator_HomingStatusChanged;
            MotionStarted -= RobotControllerQueue_MotionStarted;
            MotionEnded -= RobotControllerQueue_MotionEnded;
        }

        private void RobotSimulator_DigitalInputReceived(object sender, IOEventArgs e)
        {
            isPaused = false;
        }

        private void RobotSimulator_HomingStatusChanged(object sender, HomingStatusEventArgs e)
        {
            isPaused = false;
        }

        private void RobotSimulator_InitializationCompleted(object sender, InitializationCompletedEventArgs e)
        {
            isPaused = false;
        }

        /// <summary>
        /// Handles the MotionStarted event of the RobotController control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="RSD.Robot.MotionEventArgs"/> instance containing the event data.</param>
        private void RobotControllerQueue_MotionStarted(object sender, MotionEventArgs e)
        {
            isPaused = true;
        }

        /// <summary>
        /// Handles the MotionEnded event of the RobotController control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="RSD.Robot.MotionEventArgs"/> instance containing the event data.</param>
        private void RobotControllerQueue_MotionEnded(object sender, MotionEventArgs e)
        {
            isPaused = false;
        }

        #endregion

        #region IRobotControllerAsync Members

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer, using the default SystemType.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void InitializationAsync(InitializationMode mode, Action<bool> callback = null)
        {
            InitializationAsync(mode, SystemType.Default, callback);
        }

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <param name="systemType">System type.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void InitializationAsync(InitializationMode mode, SystemType systemType, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => Initialization(mode, systemType));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Homes the robot.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void HomeAsync(Axis axis, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => Home(axis));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveJointAsync(Vector vector, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveJoint(vector));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveJointAsync(Vector vectorA, Vector vectorB, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveJoint(vectorA, vectorB));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveLinearAsync(Vector vector, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveLinear(vector));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveLinearAsync(Vector vectorA, Vector vectorB, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveLinear(vectorA, vectorB));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the conveyor belt.
        /// </summary>
        /// <param name="velocity">Speed/direction of the conveyor belt.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveConveyorBeltAsync(int velocity, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveConveyorBelt(velocity));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Opens the gripper.
        /// </summary>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void OpenGripperAsync(Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => OpenGripper());

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Closes the gripper.
        /// </summary>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void CloseGripperAsync(Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => CloseGripper());

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        #endregion
    }

#pragma warning restore 67
}
